﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.IO;

namespace RehabilitationRobot
{
    public class RecoveryModule
    {
        #region 标准步态
        #region 标准步态数组声明
        internal double[,] NormalStepStartData = new double[4, 200];
        internal double[,] NormalStepPeriodData = new double[4, 150];
        internal double[] NormalStepStartVel = new double[200];
        internal double[] NormalStepPeriodVel = new double[150];
        #endregion

        internal void NormalStep()
        {
            /* 横坐标拉伸系数 */
            double Str_change = 1 / 6.0;
            /* 纵坐标拉伸系数 */
            double Hgh_change = 0.6;
            /* 周期插补点数150 */
            const int InterpNum = 150;
            /* 起步数据存放数组（角度、速度、加速度）*4 */
            double[,] StartData = new double[12, 200];
            /* 周期数据存放数组 */
            double[,] PeriodData = new double[12, 150];

            #region 髋关节起步阶段计算过程
            #region 髋关节起步阶段插补关节系数
            double HipStartLeft_a1 = 13.6549;//左髋关节拟合各项系数，中间点数据选择不一样拟合数据会有所差异
            double HipStartLeft_b1 = -23.9057;
            double HipStartLeft_c1 = 0.0;
            double HipStartLeft_d1 = 0.0;
            double HipStartLeft_a2 = -9.9137;
            double HipStartLeft_b2 = 17.0590;
            double HipStartLeft_c2 = -6.8467;
            double HipStartLeft_d2 = -10.2508;
            double HipStartRight_a1 = -19.1614; //右髋关节拟合各项系数，中间点数据选择不一样拟合数据会有所差异
            double HipStartRight_b1 = 34.3692;
            double HipStartRight_c1 = 0.0;
            double HipStartRight_d1 = 0.0;
            double HipStartRight_a2 = 11.4178;
            double HipStartRight_b2 = -23.1150;
            double HipStartRight_c2 = 11.2542;
            double HipStartRight_d2 = 15.2078;
            #endregion

            #region 计算过程
            /* 0至1秒插补 */
            for (int i = 0; i <= 100; i++)
            {
                double time = (double)i / 100.0;  //当前时刻更新
                StartData[0, i] = HipStartLeft_a1 * time * time * time + HipStartLeft_b1 * time * time + HipStartLeft_c1 * time + HipStartLeft_d1;
                //建立左髋关节角度数学模型，并定时插补（0-1秒插补点个数为100）

                StartData[1, i] = 3 * HipStartLeft_a1 * time * time + 2 * HipStartLeft_b1 * time + HipStartLeft_c1;
                //建立左髋关节速度数学模型，并定时插补（0-1秒插补点个数为100）

                StartData[2, i] = 3 * 2 * HipStartLeft_a1 * time + 2 * HipStartLeft_b1;
                //建立左髋关节加速度数学模型，并定时插补（0-1秒插补点个数为100）

                StartData[3, i] = HipStartRight_a1 * time * time * time + HipStartRight_b1 * time * time + HipStartRight_c1 * time + HipStartRight_d1;
                //建立右髋关节角度数学模型，并定时插补（0-1秒插补点个数为100）

                StartData[4, i] = 3 * HipStartRight_a1 * time * time + 2 * HipStartRight_b1 * time + HipStartRight_c1;
                //建立右髋关节速度数学模型，并定时插补（0-1秒插补点个数为100）

                StartData[5, i] = 3 * 2 * HipStartRight_a1 * time + 2 * HipStartRight_b1;
                //建立右髋关节加速度数学模型，并定时插补（0-1秒插补点个数为100）
            }

            /* 1至2秒插补 */
            for (int j = 100; j < 200; j++)
            {
                double time = (double)j / 100.0;  //当前时刻更新
                StartData[0, j] = HipStartLeft_a2 * (time - 1) * (time - 1) * (time - 1) + HipStartLeft_b2 * (time - 1) * (time - 1) + HipStartLeft_c2 * (time - 1) + HipStartLeft_d2;
                //建立左髋关节角度数学模型，并定时插补（1-2秒插补点个数为100）

                StartData[1, j] = 3 * HipStartLeft_a2 * (time - 1) * (time - 1) + 2 * HipStartLeft_b2 * (time - 1) + HipStartLeft_c2;
                //建立左髋关节速度数学模型，并定时插补（1-2秒插补点个数为100）

                StartData[2, j] = 3 * 2 * HipStartLeft_a2 * (time - 1) + 2 * HipStartLeft_b2;
                //建立左髋关节加速度数学模型，并定时插补（1-2秒插补点个数为100）

                StartData[3, j] = HipStartRight_a2 * (time - 1) * (time - 1) * (time - 1) + HipStartRight_b2 * (time - 1) * (time - 1) + HipStartRight_c2 * (time - 1) + HipStartRight_d2;
                //建立右髋关节角度数学模型，并定时插补（1-2秒插补点个数为100）

                StartData[4, j] = 3 * HipStartRight_a2 * (time - 1) * (time - 1) + 2 * HipStartRight_b2 * (time - 1) + HipStartRight_c2;
                //建立右髋关节速度数学模型，并定时插补（1-2秒插补点个数为100）

                StartData[5, j] = 3 * 2 * HipStartRight_a2 * (time - 1) + 2 * HipStartRight_b2;
                //建立右髋关节加速度数学模型，并定时插补（1-2秒插补点个数为100）
            }
            #endregion
            #endregion

            #region 膝关节起步阶段计算过程
            #region 膝关节起步阶段插补关节系数
            double KneeStartLeft_a1 = 17.1552;//左膝关节拟合各项系数，中间点数据选择不一样拟合数据会有所差异
            double KneeStartLeft_b1 = -25.1942;
            double KneeStartLeft_c1 = 0.0;
            double KneeStartLeft_d1 = 0.0;
            double KneeStartLeft_a2 = -24.2025;
            double KneeStartLeft_b2 = 26.2715;
            double KneeStartLeft_c2 = 1.0773;
            double KneeStartLeft_d2 = -8.0390;

            double KneeStartRight_a1 = 22.1587;//右膝关节拟合各项系数，中间点数据选择不一样拟合数据会有所差异
            double KneeStartRight_b1 = -32.6021;
            double KneeStartRight_c1 = 0.0;
            double KneeStartRight_d1 = 0.0;
            double KneeStartRight_a2 = -27.0749;
            double KneeStartRight_b2 = 33.8741;
            double KneeStartRight_c2 = 1.2719;
            double KneeStartRight_d2 = -10.4434;
            #endregion

            #region 计算过程
            /* 0至1秒插补 */
            for (int k = 0; k < 100; k++)
            {
                double time = (double)k / 100.0;  //当前时刻更新
                StartData[6, k] = KneeStartLeft_a1 * time * time * time + KneeStartLeft_b1 * time * time + KneeStartLeft_c1 * time + KneeStartLeft_d1;
                //建立左膝关节角度数学模型，并定时插补（0-1秒插补点个数为100）

                StartData[7, k] = 3 * KneeStartLeft_a1 * time * time + 2 * KneeStartLeft_b1 * time + KneeStartLeft_c1;
                //建立左膝关节速度数学模型，并定时插补（0-1秒插补点个数为100）

                StartData[8, k] = 3 * 2 * KneeStartLeft_a1 * time + 2 * KneeStartLeft_b1;
                //建立左膝关节加速度数学模型，并定时插补（0-1秒插补点个数为100）

                StartData[9, k] = KneeStartRight_a1 * time * time * time + KneeStartRight_b1 * time * time + KneeStartRight_c1 * time + KneeStartRight_d1;
                //建立右膝关节角度数学模型，并定时插补（0-1秒插补点个数为100）

                StartData[10, k] = 3 * KneeStartRight_a1 * time * time + 2 * KneeStartRight_b1 * time + KneeStartRight_c1;
                //建立右膝关节速度数学模型，并定时插补（0-1秒插补点个数为100）

                StartData[11, k] = 3 * 2 * KneeStartRight_a1 * time + 2 * KneeStartRight_b1;
                //建立右膝关节加速度数学模型，并定时插补（0-1秒插补点个数为100）
            }

            /* 1至2秒插补 */
            for (int t = 100; t < 200; t++)
            {
                double time = (double)t / 100.0;  //当前时刻更新
                StartData[6, t] = KneeStartLeft_a2 * (time - 1) * (time - 1) * (time - 1) + KneeStartLeft_b2 * (time - 1) * (time - 1) + KneeStartLeft_c2 * (time - 1) + KneeStartLeft_d2;
                //建立左膝关节角度数学模型，并定时插补（1-2秒插补点个数为100）

                StartData[7, t] = 3 * KneeStartLeft_a2 * (time - 1) * (time - 1) + 2 * KneeStartLeft_b2 * (time - 1) + HipStartLeft_c2;
                //建立左膝关节速度数学模型，并定时插补（1-2秒插补点个数为100）

                StartData[8, t] = 3 * 2 * KneeStartLeft_a2 * (time - 1) + 2 * KneeStartLeft_b2;
                //建立左膝关节加速度数学模型，并定时插补（1-2秒插补点个数为100）

                StartData[9, t] = KneeStartRight_a2 * (time - 1) * (time - 1) * (time - 1) + KneeStartRight_b2 * (time - 1) * (time - 1) + KneeStartRight_c2 * (time - 1) + KneeStartRight_d2;
                //建立右膝关节角度数学模型，并定时插补（1-2秒插补点个数为100）

                StartData[10, t] = 3 * KneeStartRight_a2 * (time - 1) * (time - 1) + 2 * KneeStartRight_b2 * (time - 1) + KneeStartRight_c2;
                //建立右膝关节速度数学模型，并定时插补（1-2秒插补点个数为100）

                StartData[11, t] = 3 * 2 * KneeStartRight_a2 * (time - 1) + 2 * KneeStartRight_b2;
                //建立右膝关节加速度数学模型，并定时插补（1-2秒插补点个数为100）
            }
            #endregion
            #endregion

            #region 周期阶段
            #region 拟合关节系数
            double HipPeriodRight_a1 = 30.59; //右髋关节拟合各项系数
            double HipPeriodRight_b1 = 6.051;
            double HipPeriodRight_c1 = 1.801;
            double HipPeriodRight_a2 = 16.29;
            double HipPeriodRight_b2 = 3.492;
            double HipPeriodRight_c2 = -0.1865;
            double HipPeriodRight_a3 = 3.096;
            double HipPeriodRight_b3 = 14.26;
            double HipPeriodRight_c3 = -2.73;
            double HipPeriodRight_a4 = 1.431;
            double HipPeriodRight_b4 = 19.35;
            double HipPeriodRight_c4 = -0.4474;
            double HipPeriodRight_a5 = 0.08455;
            double HipPeriodRight_b5 = 42.81;
            double HipPeriodRight_c5 = -1.464;
            double HipPeriodRight_a6 = 1.059;
            double HipPeriodRight_b6 = 30.69;
            double HipPeriodRight_c6 = -1.186;
            double HipPeriodRight_a7 = 0.9797;
            double HipPeriodRight_b7 = 29.61;
            double HipPeriodRight_c7 = 2.239;

            double KneePeriodRight_a1 = 35.12;//右膝关节拟合各项系数
            double KneePeriodRight_b1 = 3.386;
            double KneePeriodRight_c1 = 2.933;
            double KneePeriodRight_a2 = 60.3;
            double KneePeriodRight_b2 = 15.07;
            double KneePeriodRight_c2 = 1.413;
            double KneePeriodRight_a3 = 25.16;
            double KneePeriodRight_b3 = 6.671;
            double KneePeriodRight_c3 = -0.9683;
            double KneePeriodRight_a4 = 53.61;
            double KneePeriodRight_b4 = 15.64;
            double KneePeriodRight_c4 = -1.887;
            double KneePeriodRight_a5 = 0.141;
            double KneePeriodRight_b5 = 42.26;
            double KneePeriodRight_c5 = -4.511;
            double KneePeriodRight_a6 = 2.513;
            double KneePeriodRight_b6 = 30.79;
            double KneePeriodRight_c6 = -4.109;
            double KneePeriodRight_a7 = 2.481;
            double KneePeriodRight_b7 = 29.46;
            double KneePeriodRight_c7 = -6.836;
            #endregion

            #region 计算过程
            for (int i = 0; i < InterpNum; i++)
            {
                double time = (double)i * 6.0 / (double)InterpNum;  //当前时刻更新
                PeriodData[3, i] = Hgh_change * (
                      HipPeriodRight_a1 * Math.Sin(HipPeriodRight_b1 * Str_change * time + HipPeriodRight_c1) + HipPeriodRight_a2 * Math.Sin(HipPeriodRight_b2 * Str_change * time + HipPeriodRight_c2) + HipPeriodRight_a3 * Math.Sin(HipPeriodRight_b3 * Str_change * time + HipPeriodRight_c3)
                    + HipPeriodRight_a4 * Math.Sin(HipPeriodRight_b4 * Str_change * time + HipPeriodRight_c4) + HipPeriodRight_a5 * Math.Sin(HipPeriodRight_b5 * Str_change * time + HipPeriodRight_c5) + HipPeriodRight_a6 * Math.Sin(HipPeriodRight_b6 * Str_change * time + HipPeriodRight_c6)
                    + HipPeriodRight_a7 * Math.Sin(HipPeriodRight_b7 * Str_change * time + HipPeriodRight_c7));       //建立右髋关节角度数学模型，并定时插补（插补点个数InterpNum为100）

                PeriodData[4, i] = Hgh_change * (
                      HipPeriodRight_b1 * Str_change * HipPeriodRight_a1 * Math.Cos(HipPeriodRight_b1 * Str_change * time + HipPeriodRight_c1) + HipPeriodRight_b2 * Str_change * HipPeriodRight_a2 * Math.Cos(HipPeriodRight_b2 * Str_change * time + HipPeriodRight_c2) + HipPeriodRight_b3 * Str_change * HipPeriodRight_a3 * Math.Cos(HipPeriodRight_b3 * Str_change * time + HipPeriodRight_c3)
                    + HipPeriodRight_b4 * Str_change * HipPeriodRight_a4 * Math.Cos(HipPeriodRight_b4 * Str_change * time + HipPeriodRight_c4) + HipPeriodRight_b5 * Str_change * HipPeriodRight_a5 * Math.Cos(HipPeriodRight_b5 * Str_change * time + HipPeriodRight_c5) + HipPeriodRight_b6 * Str_change * HipPeriodRight_a6 * Math.Cos(HipPeriodRight_b6 * Str_change * time + HipPeriodRight_c6)
                    + HipPeriodRight_b7 * Str_change * HipPeriodRight_a7 * Math.Cos(HipPeriodRight_b7 * Str_change * time + HipPeriodRight_c7));  //建立右髋关节速度数学模型，并定时插补（插补点个数InterpNum为100）

                PeriodData[5, i] = -1.0 * Hgh_change * (
                      KneePeriodRight_b1 * KneePeriodRight_b1 * Str_change * Str_change * KneePeriodRight_a1 * Math.Sin(KneePeriodRight_b1 * Str_change * time + KneePeriodRight_c1) + KneePeriodRight_b2 * KneePeriodRight_b2 * Str_change * Str_change * KneePeriodRight_a2 * Math.Sin(KneePeriodRight_b2 * Str_change * time + KneePeriodRight_c2) + KneePeriodRight_b3 * KneePeriodRight_b3 * Str_change * Str_change * KneePeriodRight_a3 * Math.Sin(KneePeriodRight_b3 * Str_change * time + KneePeriodRight_c3)
                    + KneePeriodRight_b4 * KneePeriodRight_b4 * Str_change * Str_change * KneePeriodRight_a4 * Math.Sin(KneePeriodRight_b4 * Str_change * time + KneePeriodRight_c4) + KneePeriodRight_b5 * KneePeriodRight_b5 * Str_change * Str_change * KneePeriodRight_a5 * Math.Sin(KneePeriodRight_b5 * Str_change * time + KneePeriodRight_c5) + KneePeriodRight_b6 * KneePeriodRight_b6 * Str_change * Str_change * KneePeriodRight_a6 * Math.Sin(KneePeriodRight_b6 * Str_change * time + KneePeriodRight_c6)
                    + KneePeriodRight_b7 * KneePeriodRight_b7 * Str_change * Str_change * KneePeriodRight_a7 * Math.Sin(KneePeriodRight_b7 * Str_change * time + KneePeriodRight_c7)); //建立右髋关节加速度数学模型，并定时插补（插补点个数InterpNum为100）

                PeriodData[9, i] = Hgh_change * (
                      KneePeriodRight_a1 * Math.Sin(KneePeriodRight_b1 * Str_change * time + KneePeriodRight_c1) + KneePeriodRight_a2 * Math.Sin(KneePeriodRight_b2 * Str_change * time + KneePeriodRight_c2) + KneePeriodRight_a3 * Math.Sin(KneePeriodRight_b3 * Str_change * time + KneePeriodRight_c3)
                    + KneePeriodRight_a4 * Math.Sin(KneePeriodRight_b4 * Str_change * time + KneePeriodRight_c4) + KneePeriodRight_a5 * Math.Sin(KneePeriodRight_b5 * Str_change * time + KneePeriodRight_c5) + KneePeriodRight_a6 * Math.Sin(KneePeriodRight_b6 * Str_change * time + KneePeriodRight_c6)
                    + KneePeriodRight_a7 * Math.Sin(KneePeriodRight_b7 * Str_change * time + KneePeriodRight_c7));//建立右膝关节角度数学模型，并定时插补（插补点个数InterpNum为100）

                PeriodData[10, i] = Hgh_change * (
                     KneePeriodRight_b1 * Str_change * KneePeriodRight_a1 * Math.Cos(KneePeriodRight_b1 * Str_change * time + KneePeriodRight_c1) + KneePeriodRight_b2 * Str_change * KneePeriodRight_a2 * Math.Cos(KneePeriodRight_b2 * Str_change * time + KneePeriodRight_c2) + KneePeriodRight_b3 * Str_change * KneePeriodRight_a3 * Math.Cos(KneePeriodRight_b3 * Str_change * time + KneePeriodRight_c3)
                   + KneePeriodRight_b4 * Str_change * KneePeriodRight_a4 * Math.Cos(KneePeriodRight_b4 * Str_change * time + KneePeriodRight_c4) + KneePeriodRight_b5 * Str_change * KneePeriodRight_a5 * Math.Cos(KneePeriodRight_b5 * Str_change * time + KneePeriodRight_c5) + KneePeriodRight_b6 * Str_change * KneePeriodRight_a6 * Math.Cos(KneePeriodRight_b6 * Str_change * time + KneePeriodRight_c6)
                   + KneePeriodRight_b7 * Str_change * KneePeriodRight_a7 * Math.Cos(KneePeriodRight_b7 * Str_change * time + KneePeriodRight_c7));  //建立右膝关节速度数学模型，并定时插补（插补点个数InterpNum为100）

                PeriodData[11, i] = -1.0 * Hgh_change * (
                     KneePeriodRight_b1 * KneePeriodRight_b1 * Str_change * Str_change * KneePeriodRight_a1 * Math.Sin(KneePeriodRight_b1 * Str_change * time + KneePeriodRight_c1) + KneePeriodRight_b2 * KneePeriodRight_b2 * Str_change * Str_change * KneePeriodRight_a2 * Math.Sin(KneePeriodRight_b2 * Str_change * time + KneePeriodRight_c2) + KneePeriodRight_b3 * KneePeriodRight_b3 * Str_change * Str_change * KneePeriodRight_a3 * Math.Sin(KneePeriodRight_b3 * Str_change * time + KneePeriodRight_c3)
                   + KneePeriodRight_b4 * KneePeriodRight_b4 * Str_change * Str_change * KneePeriodRight_a4 * Math.Sin(KneePeriodRight_b4 * Str_change * time + KneePeriodRight_c4) + KneePeriodRight_b5 * KneePeriodRight_b5 * Str_change * Str_change * KneePeriodRight_a5 * Math.Sin(KneePeriodRight_b5 * Str_change * time + KneePeriodRight_c5) + KneePeriodRight_b6 * KneePeriodRight_b6 * Str_change * Str_change * KneePeriodRight_a6 * Math.Sin(KneePeriodRight_b6 * Str_change * time + KneePeriodRight_c6)
                   + KneePeriodRight_b7 * KneePeriodRight_b7 * Str_change * Str_change * KneePeriodRight_a7 * Math.Sin(KneePeriodRight_b7 * Str_change * time + KneePeriodRight_c7));   //建立右膝关节加速度数学模型，并定时插补（插补点个数InterpNum为100）
            }

            for (int j = 0; j < InterpNum; j++)
            {
                if (j < (InterpNum / 2))
                {
                    //左髋、膝关节前50%步态周期运动数据与右髋、膝关节后50%步态周期运动数据相同（步态对称性）
                    PeriodData[0, j] = PeriodData[3, j + InterpNum / 2];//左髋关节角度数据与右髋关节角度数据相差50%步态周期
                    PeriodData[1, j] = PeriodData[4, j + InterpNum / 2];//左髋关节速度数据与右髋关节速度数据相差50%步态周期
                    PeriodData[2, j] = PeriodData[5, j + InterpNum / 2];//左髋关节加速度数据与右髋关节加速度数据相差50%步态周期
                    PeriodData[6, j] = PeriodData[9, j + InterpNum / 2];//左膝关节角度数据与右髋关节角度数据相差50%步态周期
                    PeriodData[7, j] = PeriodData[10, j + InterpNum / 2];//左膝关节速度数据与右髋关节速度数据相差50%步态周期
                    PeriodData[8, j] = PeriodData[11, j + InterpNum / 2];//左膝关节加速度数据与右髋关节加速度数据相差50%步态周期
                }
                else
                {
                    //左髋、膝关节后50%步态周期运动数据与右髋、膝关节前50%步态周期运动数据相同（步态对称性）
                    PeriodData[0, j] = PeriodData[3, j - InterpNum / 2];//左髋关节角度数据与右髋关节角度数据相差50%步态周期
                    PeriodData[1, j] = PeriodData[4, j - InterpNum / 2];//左髋关节速度数据与右髋关节速度数据相差50%步态周期
                    PeriodData[2, j] = PeriodData[5, j - InterpNum / 2];//左髋关节加速度数据与右髋关节加速度数据相差50%步态周期
                    PeriodData[6, j] = PeriodData[9, j - InterpNum / 2];//左膝关节角度数据与右髋关节角度数据相差50%步态周期
                    PeriodData[7, j] = PeriodData[10, j - InterpNum / 2];//左膝关节速度数据与右髋关节速度数据相差50%步态周期
                    PeriodData[8, j] = PeriodData[11, j - InterpNum / 2];//左膝关节加速度数据与右髋关节加速度数据相差50%步态周期
                }
            }
            #endregion

            #endregion

            #region  打印到txt
            string path = @"..\..\..\JiontTxt\NomalStep.txt";
            File.Delete(path);
            FileStream input = new FileStream(path, FileMode.Append);
            StreamWriter output = null;
            output = new StreamWriter(input);
            output.WriteLine("阶段" + "，" + "左髋" + "，" + "右髋" + "，" + "左膝" + "，" + "右膝" + "，" + "速度");
            #endregion

            #region 轨迹生成
            /* 起步轨迹 */
            for (int i = 0; i < 200; i++)
            {
                NormalStepStartVel[i] = (float)Math.Sqrt(
                  StartData[1, i] * StartData[1, i]
                + StartData[4, i] * StartData[4, i]
                + StartData[7, i] * StartData[7, i]
                + StartData[10, i] * StartData[10, i]
                ) / 4;
                for (int j = 0; j < 4; j++)
                {
                    NormalStepStartData[j, i] = StartData[3 * j, i];
                }
                output.WriteLine("起步{0}" + "，" + NormalStepStartData[0, i].ToString("F4") + "，" +
                    NormalStepStartData[1, i].ToString("F4") + "，" + NormalStepStartData[2, i].ToString("F4") + "，" +
                    NormalStepStartData[3, i].ToString("F4") + "，" + NormalStepStartVel[i].ToString("F4"), i);
            }
            /* 周期轨迹 */
            for (int i = 0; i < 150; i++)
            {
                NormalStepPeriodVel[i] = (float)Math.Sqrt(
                  PeriodData[1, i] * PeriodData[1, i]
                + PeriodData[4, i] * PeriodData[4, i]
                + PeriodData[7, i] * PeriodData[7, i]
                + PeriodData[10, i] * PeriodData[10, i]
                ) / 4;
                for (int j = 0; j < 4; j++)
                {
                    NormalStepPeriodData[j, i] = PeriodData[3 * j, i];
                }
                output.WriteLine("周期{0}" + "，" + NormalStepPeriodData[0, i].ToString("F4") + "，" +
                    NormalStepPeriodData[1, i].ToString("F4") + "，" + NormalStepPeriodData[2, i].ToString("F4") + "，" +
                    NormalStepPeriodData[3, i].ToString("F4") + "，" + NormalStepPeriodVel[i].ToString("F4"), i);
            }
            output.Close();
            #endregion
        }
        #endregion

        #region 小圆弧模式
        #region 小圆弧模式数组声明
        internal double[,] SmallArcStartData = new double[4, 75];
        internal double[,] SmallArcPeriodData = new double[4, 150];
        internal double[] SmallArcStartVel = new double[75];
        internal double[] SmallArcPeriodVel = new double[150];
        #endregion

        internal void SmallArc()
        {
            /* 周期插补点数150 */
            const int InterpNum = 150;
            /* 轨迹生成函数 */
            double[,] Trajectory = new double[6, 150];
            /* 起步数组 */
            double[,] StartData = new double[12, 75];
            /* 周期运动数组 */
            double[,] PeriodData = new double[12, 150];

            #region  拟合关节系数
            double HipFactor1 = 23.13;
            double HipFactor2 = 0.05558;
            double HipFactor3 = 0.4593;
            double HipFactor4 = 7.832;
            double HipFactor5 = 0.1846;
            double HipFactor6 = -2.122;
            double HipFactor7 = 3.846;
            double HipFactor8 = 0.1587;
            double HipFactor9 = 4.678;

            double KneeFactor1 = -23.13;
            double KneeFactor2 = -0.05558;
            double KneeFactor3 = -0.4593;
            double KneeFactor4 = -7.832;
            double KneeFactor5 = -0.1846;
            double KneeFactor6 = 2.122;
            double KneeFactor7 = -3.846;
            double KneeFactor8 = -0.1587;
            double KneeFactor9 = -4.678;

            #endregion

            #region 计算过程
            for (int i = 0; i < InterpNum; i++)
            {
                double time = (double)i * 40 / (double)InterpNum;
                Trajectory[0, i] = (
                HipFactor1 * Math.Sin(HipFactor2 * time + HipFactor3) + HipFactor4 * Math.Sin(HipFactor5 * time + HipFactor6) + HipFactor7 * Math.Sin(HipFactor8 * time + HipFactor9));
                //建立髋关节角度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[1, i] = (
                 HipFactor2 * HipFactor1 * Math.Cos(HipFactor2 * time + HipFactor3) + HipFactor5 * HipFactor4 * Math.Cos(HipFactor5 * time + HipFactor6) + HipFactor8 * HipFactor7 * Math.Cos(HipFactor8 * time + HipFactor9))
                ;//建立髋关节速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[2, i] = -1.0 * (
                 HipFactor2 * HipFactor2 * HipFactor1 * Math.Sin(HipFactor2 * time + HipFactor3) + HipFactor5 * HipFactor5 * HipFactor4 * Math.Sin(HipFactor5 * time + HipFactor6) + HipFactor8 * HipFactor8 * HipFactor7 * Math.Sin(HipFactor8 * time + HipFactor9))
                ; //建立髋关节加速度数学模型，并定时插补（插补点个数Standardgait_internumber为100） 

                Trajectory[3, i] = (
                KneeFactor1 * Math.Sin(KneeFactor2 * time + KneeFactor3) + KneeFactor4 * Math.Sin(KneeFactor5 * time + KneeFactor6) + KneeFactor7 * Math.Sin(KneeFactor8 * time + KneeFactor9));
                //建立膝关节角度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[4, i] = (
                 KneeFactor2 * KneeFactor1 * Math.Cos(KneeFactor2 * time + KneeFactor3) + KneeFactor5 * KneeFactor4 * Math.Cos(KneeFactor5 * time + KneeFactor6) + KneeFactor8 * KneeFactor7 * Math.Cos(KneeFactor8 * time + KneeFactor9))
                ;//建立膝关节速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[5, i] = -1.0 * (
                 KneeFactor2 * KneeFactor2 * KneeFactor1 * Math.Sin(KneeFactor2 * time + KneeFactor3) + KneeFactor5 * KneeFactor5 * KneeFactor4 * Math.Sin(KneeFactor5 * time + KneeFactor6) + KneeFactor8 * KneeFactor8 * KneeFactor7 * Math.Sin(KneeFactor8 * time + KneeFactor9))
                ; //建立膝关节加速度数学模型，并定时插补（插补点个数Standardgait_internumber为100） 
            }

            for (int i = 0; i < 75; i++)
            {
                StartData[0, i] = 0;//左髋
                StartData[1, i] = 0;
                StartData[2, i] = 0;
                StartData[3, i] = Trajectory[0, i];//右髋
                StartData[4, i] = Trajectory[1, i];
                StartData[5, i] = Trajectory[2, i];
                StartData[6, i] = 0;//左膝
                StartData[7, i] = 0;
                StartData[8, i] = 0;
                if (Trajectory[3, i] > 0)
                {
                    StartData[9, i] = 0;
                }
                else
                {
                    StartData[9, i] = Trajectory[3, i];//右膝
                }
                StartData[10, i] = Trajectory[4, i];
                StartData[11, i] = Trajectory[5, i];
            }

            for (int k = 0; k < InterpNum; k++)
            {
                PeriodData[0, k] = Trajectory[0, k];
                PeriodData[1, k] = Trajectory[1, k];
                PeriodData[2, k] = Trajectory[2, k];
                if (Trajectory[3, k] >= 0)
                {
                    PeriodData[6, k] = 0;
                }
                else
                {
                    PeriodData[6, k] = Trajectory[3, k];
                }
                PeriodData[7, k] = Trajectory[4, k];
                PeriodData[8, k] = Trajectory[5, k];

                if (k < 75)
                {
                    PeriodData[3, k] = Trajectory[0, k + 75];
                    PeriodData[4, k] = Trajectory[1, k + 75];
                    PeriodData[5, k] = Trajectory[2, k + 75];
                    if (Trajectory[3, k + 75] > 0)
                    {
                        PeriodData[9, k] = 0;
                    }
                    else
                    {
                        PeriodData[9, k] = Trajectory[3, k + 75];
                    }
                    PeriodData[10, k] = Trajectory[4, k + 75];
                    PeriodData[11, k] = Trajectory[5, k + 75];
                }
                else
                {
                    PeriodData[3, k] = Trajectory[0, k - 75];
                    PeriodData[4, k] = Trajectory[1, k - 75];
                    PeriodData[5, k] = Trajectory[2, k - 75];
                    if (Trajectory[3, k - 75] > 0)
                    {
                        PeriodData[9, k] = 0;
                    }
                    else
                    {
                        PeriodData[9, k] = Trajectory[3, k - 75];
                    }

                    PeriodData[10, k] = Trajectory[4, k - 75];
                    PeriodData[11, k] = Trajectory[5, k - 75];
                }
            }
            #endregion

            #region  打印到txt
            string path = @"..\..\..\JiontTxt\SmallArc.txt";
            File.Delete(path);
            FileStream input = new FileStream(path, FileMode.Append);
            StreamWriter output = null;
            output = new StreamWriter(input);
            output.WriteLine("阶段" + "，" + "左髋" + "，" + "右髋" + "，" + "左膝" + "，" + "右膝" + "，" + "速度");
            #endregion

            #region 轨迹生成
            /* 起步轨迹 */
            for (int i = 0; i < 75; i++)
            {
                SmallArcStartVel[i] = (float)Math.Sqrt(
                  StartData[1, i] * StartData[1, i]
                + StartData[4, i] * StartData[4, i]
                + StartData[7, i] * StartData[7, i]
                + StartData[10, i] * StartData[10, i]
                ) / 1.5;
                for (int j = 0; j < 4; j++)
                {
                    SmallArcStartData[j, i] = StartData[3 * j, i];
                }
                output.WriteLine("起步{0}" + "，" + SmallArcStartData[0, i].ToString("F4") + "，" +
                    SmallArcStartData[1, i].ToString("F4") + "，" + SmallArcStartData[2, i].ToString("F4") + "，" +
                    SmallArcStartData[3, i].ToString("F4") + "，" + SmallArcStartVel[i].ToString("F4"), i);
            }
            /* 周期轨迹 */
            for (int i = 0; i < 150; i++)
            {
                SmallArcPeriodVel[i] = (float)Math.Sqrt(
                  PeriodData[1, i] * PeriodData[1, i]
                + PeriodData[4, i] * PeriodData[4, i]
                + PeriodData[7, i] * PeriodData[7, i]
                + PeriodData[10, i] * PeriodData[10, i]
                ) / 1.5;
                for (int j = 0; j < 4; j++)
                {
                    SmallArcPeriodData[j, i] = PeriodData[3 * j, i];
                }
                output.WriteLine("周期{0}" + "，" + SmallArcPeriodData[0, i].ToString("F4") + "，" +
                    SmallArcPeriodData[1, i].ToString("F4") + "，" + SmallArcPeriodData[2, i].ToString("F4") + "，" +
                    SmallArcPeriodData[3, i].ToString("F4") + "，" + SmallArcPeriodVel[i].ToString("F4"), i);
            }
            output.Close();
            #endregion
        }
        #endregion

        #region 直线训练模式
        #region 直线训练模式数组声明
        internal double[,] LinearStartData = new double[4, 75];
        internal double[,] LinearPeriodData = new double[4, 150];
        internal double[] LinearStartVel = new double[75];
        internal double[] LinearPeriodVel = new double[150];
        #endregion

        internal void Linear()
        {
            /* 周期插补点数150 */
            const int InterpNum = 150;
            /* 轨迹生成函数 */
            double[,] Trajectory = new double[6, 150];
            /* 起步数组 */
            double[,] StartData = new double[12, 75];
            /* 周期运动数组 */
            double[,] PeriodData = new double[12, 150];

            #region 插补系数
            double HipFactor_a1 = 33.99;
            double HipFactor_b1 = 0.1049;
            double HipFactor_c1 = -0.002555;
            double HipFactor_a2 = 0.2596;
            double HipFactor_b2 = 0.2219;
            double HipFactor_c2 = 1.388;
            double HipFactor_a3 = 0.1641;
            double HipFactor_b3 = 0.6204;
            double HipFactor_c3 = -1.456;
            double HipFactor_a4 = 0.0778;
            double HipFactor_b4 = 0.8109;
            double HipFactor_c4 = -1.456;
            double HipFactor_a5 = 0.07267;
            double HipFactor_b5 = 0.9987;
            double HipFactor_c5 = -0.8533;
            double HipFactor_a6 = 0.5308;
            double HipFactor_b6 = 1.192;
            double HipFactor_c6 = -0.5978;
            double HipFactor_a7 = 0.5047;
            double HipFactor_b7 = 1.197;
            double HipFactor_c7 = 2.473;

            double KneeFactor_a1 = 77.79;
            double KneeFactor_b1 = 0.1047;
            double KneeFactor_c1 = -3.141;
            double KneeFactor_a2 = 0.3116;
            double KneeFactor_b2 = 0.4234;
            double KneeFactor_c2 = -1.633;
            double KneeFactor_a3 = 0.276;
            double KneeFactor_b3 = 0.628;
            double KneeFactor_c3 = 1.571;
            double KneeFactor_a4 = 0.09125;
            double KneeFactor_b4 = 1.047;
            double KneeFactor_c4 = 1.571;
            double KneeFactor_a5 = 0.07805;
            double KneeFactor_b5 = 0.8376;
            double KneeFactor_c5 = 1.571;
            double KneeFactor_a6 = 0.03875;
            double KneeFactor_b6 = 0.1946;
            double KneeFactor_c6 = -1.348;
            double KneeFactor_a7 = 0.04609;
            double KneeFactor_b7 = 1.256;
            double KneeFactor_c7 = 1.571;
            #endregion

            #region 计算过程
            for (int i = 0; i < InterpNum; i++)
            {
                double time = (double)i * 30 / (double)InterpNum;  //当前时刻更新

                Trajectory[0, i] = (
                      HipFactor_a1 * Math.Sin(HipFactor_b1 * time + HipFactor_c1) + HipFactor_a2 * Math.Sin(HipFactor_b2 * time + HipFactor_c2) + HipFactor_a3 * Math.Sin(HipFactor_b3 * time + HipFactor_c3)
                    + HipFactor_a4 * Math.Sin(HipFactor_b4 * time + HipFactor_c4) + HipFactor_a5 * Math.Sin(HipFactor_b5 * time + HipFactor_c5) + HipFactor_a6 * Math.Sin(HipFactor_b6 * time + HipFactor_c6)
                    + HipFactor_a7 * Math.Sin(HipFactor_b7 * time + HipFactor_c7));
                //建立髋关节角度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[1, i] = (
                      HipFactor_b1 * HipFactor_a1 * Math.Cos(HipFactor_b1 * time + HipFactor_c1) + HipFactor_b2 * HipFactor_a2 * Math.Cos(HipFactor_b2 * time + HipFactor_c2) + HipFactor_b3 * HipFactor_a3 * Math.Cos(HipFactor_b3 * time + HipFactor_c3)
                    + HipFactor_b4 * HipFactor_a4 * Math.Cos(HipFactor_b4 * time + HipFactor_c4) + HipFactor_b5 * HipFactor_a5 * Math.Cos(HipFactor_b5 * time + HipFactor_c5) + HipFactor_b6 * HipFactor_a6 * Math.Cos(HipFactor_b6 * time + HipFactor_c6)
                    + HipFactor_b7 * HipFactor_a7 * Math.Cos(HipFactor_b7 * time + HipFactor_c7)); //建立髋关节速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[2, i] = -1.0 * (
                      HipFactor_b1 * HipFactor_b1 * HipFactor_a1 * Math.Sin(HipFactor_b1 * time + HipFactor_c1) + HipFactor_b2 * HipFactor_b2 * HipFactor_a2 * Math.Sin(HipFactor_b2 * time + HipFactor_c2) + HipFactor_b3 * HipFactor_b3 * HipFactor_a3 * Math.Sin(HipFactor_b3 * time + HipFactor_c3)
                    + HipFactor_b4 * HipFactor_b4 * HipFactor_a4 * Math.Sin(HipFactor_b4 * time + HipFactor_c4) + HipFactor_b5 * HipFactor_b5 * HipFactor_a5 * Math.Sin(HipFactor_b5 * time + HipFactor_c5) + HipFactor_b6 * HipFactor_b6 * HipFactor_a6 * Math.Sin(HipFactor_b6 * time + HipFactor_c6)
                    + HipFactor_b7 * HipFactor_b7 * HipFactor_a7 * Math.Sin(HipFactor_b7 * time + HipFactor_c7));  //建立髋关节加速度数学模型，并定时插补（插补点个数Standardgait_internumber为100） 

                Trajectory[3, i] = (
                      KneeFactor_a1 * Math.Sin(KneeFactor_b1 * time + KneeFactor_c1) + KneeFactor_a2 * Math.Sin(KneeFactor_b2 * time + KneeFactor_c2) + KneeFactor_a3 * Math.Sin(KneeFactor_b3 * time + KneeFactor_c3)
                    + KneeFactor_a4 * Math.Sin(KneeFactor_b4 * time + KneeFactor_c4) + KneeFactor_a5 * Math.Sin(KneeFactor_b5 * time + KneeFactor_c5) + KneeFactor_a6 * Math.Sin(KneeFactor_b6 * time + KneeFactor_c6)
                    + KneeFactor_a7 * Math.Sin(KneeFactor_b7 * time + KneeFactor_c7));
                //建立膝关节角度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[4, i] = (
                      KneeFactor_b1 * KneeFactor_a1 * Math.Cos(KneeFactor_b1 * time + KneeFactor_c1) + KneeFactor_b2 * KneeFactor_a2 * Math.Cos(KneeFactor_b2 * time + KneeFactor_c2) + KneeFactor_b3 * KneeFactor_a3 * Math.Cos(KneeFactor_b3 * time + KneeFactor_c3)
                    + KneeFactor_b4 * KneeFactor_a4 * Math.Cos(KneeFactor_b4 * time + KneeFactor_c4) + KneeFactor_b5 * KneeFactor_a5 * Math.Cos(KneeFactor_b5 * time + KneeFactor_c5) + KneeFactor_b6 * KneeFactor_a6 * Math.Cos(KneeFactor_b6 * time + KneeFactor_c6)
                    + KneeFactor_b7 * KneeFactor_a7 * Math.Cos(KneeFactor_b7 * time + KneeFactor_c7));  //建立膝关节速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[5, i] = -1.0 * (
                      KneeFactor_b1 * KneeFactor_b1 * KneeFactor_a1 * Math.Sin(KneeFactor_b1 * time + KneeFactor_c1) + KneeFactor_b2 * KneeFactor_b2 * KneeFactor_a2 * Math.Sin(KneeFactor_b2 * time + KneeFactor_c2) + KneeFactor_b3 * KneeFactor_b3 * KneeFactor_a3 * Math.Sin(KneeFactor_b3 * time + KneeFactor_c3)
                    + KneeFactor_b4 * KneeFactor_b4 * KneeFactor_a4 * Math.Sin(KneeFactor_b4 * time + KneeFactor_c4) + KneeFactor_b5 * KneeFactor_b5 * KneeFactor_a5 * Math.Sin(KneeFactor_b5 * time + KneeFactor_c5) + KneeFactor_b6 * KneeFactor_b6 * KneeFactor_a6 * Math.Sin(KneeFactor_b6 * time + KneeFactor_c6)
                    + KneeFactor_b7 * KneeFactor_b7 * KneeFactor_a7 * Math.Sin(KneeFactor_b7 * time + KneeFactor_c7));   //建立膝关节加速度数学模型，并定时插补（插补点个数Standardgait_internumber为100） 
            }

            for (int j = 0; j < 75; j++)
            {

                StartData[0, j] = 0;//左髋
                StartData[1, j] = 0;
                StartData[2, j] = 0;
                StartData[3, j] = Trajectory[0, j];//右髋
                StartData[4, j] = Trajectory[1, j];
                StartData[5, j] = Trajectory[2, j];
                StartData[6, j] = 0;//左膝
                StartData[7, j] = 0;
                StartData[8, j] = 0;
                if (Trajectory[3, j] > 0)
                {
                    StartData[9, j] = 0;
                }
                else
                {
                    StartData[9, j] = Trajectory[3, j];//右膝
                }
                StartData[10, j] = Trajectory[4, j];
                StartData[11, j] = Trajectory[5, j];
            }

            for (int k = 0; k < InterpNum; k++)
            {
                PeriodData[0, k] = Trajectory[0, k];
                PeriodData[1, k] = Trajectory[1, k];
                PeriodData[2, k] = Trajectory[2, k];
                if (Trajectory[3, k] > 0)
                {
                    PeriodData[6, k] = 0;
                }
                else
                {
                    PeriodData[6, k] = Trajectory[3, k];
                }
                PeriodData[7, k] = Trajectory[4, k];
                PeriodData[8, k] = Trajectory[5, k];

                if (k < 75)
                {
                    PeriodData[3, k] = Trajectory[0, k + 75];
                    PeriodData[4, k] = Trajectory[1, k + 75];
                    PeriodData[5, k] = Trajectory[2, k + 75];
                    if (Trajectory[3, k + 75] > 0)
                    {
                        PeriodData[9, k] = 0;
                    }
                    else
                    {
                        PeriodData[9, k] = Trajectory[3, k + 75];
                    }
                    PeriodData[10, k] = Trajectory[4, k + 75];
                    PeriodData[11, k] = Trajectory[5, k + 75];
                }
                else
                {
                    PeriodData[3, k] = Trajectory[0, k - 75];
                    PeriodData[4, k] = Trajectory[1, k - 75];
                    PeriodData[5, k] = Trajectory[2, k - 75];
                    if (Trajectory[3, k - 75] > 0)
                    {
                        PeriodData[9, k] = 0;
                    }
                    else
                    {
                        PeriodData[9, k] = Trajectory[3, k - 75];
                    }

                    PeriodData[10, k] = Trajectory[4, k - 75];
                    PeriodData[11, k] = Trajectory[5, k - 75];
                }

            }
            #endregion

            #region  打印到txt
            string path = @"..\..\..\JiontTxt\Linear.txt";
            File.Delete(path);
            FileStream input = new FileStream(path, FileMode.Append);
            StreamWriter output = null;
            output = new StreamWriter(input);
            output.WriteLine("阶段" + "，" + "左髋" + "，" + "右髋" + "，" + "左膝" + "，" + "右膝" + "，" + "速度");
            #endregion

            #region 轨迹生成
            for (int i = 0; i < 75; i++)
            {
                if (i < 6)
                {
                    LinearStartVel[i] = 1 + i * 1.6;
                }
                else
                {
                    LinearStartVel[i] = (float)Math.Sqrt(
                    StartData[1, i] * StartData[1, i]
                  + StartData[4, i] * StartData[4, i]
                  + StartData[7, i] * StartData[7, i]
                  + StartData[10, i] * StartData[10, i]
                  );//50为减速比
                }
                for (int j = 0; j < 4; j++)
                {
                    LinearStartData[j, i] = StartData[3 * j, i];
                }
                output.WriteLine("起步{0}" + "，" + LinearStartData[0, i].ToString("F4") + "，" +
                    LinearStartData[1, i].ToString("F4") + "，" + LinearStartData[2, i].ToString("F4") + "，" +
                    LinearStartData[3, i].ToString("F4") + "，" + LinearStartVel[i].ToString("F4"), i);
            }
            /* 周期轨迹 */
            for (int i = 0; i < 150; i++)
            {
                LinearPeriodVel[i] = (float)Math.Sqrt(
                  PeriodData[1, i] * PeriodData[1, i]
                + PeriodData[4, i] * PeriodData[4, i]
                + PeriodData[7, i] * PeriodData[7, i]
                + PeriodData[10, i] * PeriodData[10, i]
                ) / 4;
                for (int j = 0; j < 4; j++)
                {
                    LinearPeriodData[j, i] = PeriodData[3 * j, i];
                }
                output.WriteLine("周期{0}" + "，" + LinearPeriodData[0, i].ToString("F4") + "，" +
                    LinearPeriodData[1, i].ToString("F4") + "，" + LinearPeriodData[2, i].ToString("F4") + "，" +
                    LinearPeriodData[3, i].ToString("F4") + "，" + LinearPeriodVel[i].ToString("F4"), i);
            }
            output.Close();
            #endregion
        }
        #endregion

        #region 椭圆训练模式
        #region 椭圆训练模式数组声明
        internal double[,] OvalStartData = new double[4, 75];
        internal double[,] OvalPeriodData = new double[4, 150];
        internal double[] OvalStartVel = new double[75];
        internal double[] OvalPeriodVel = new double[150];
        #endregion

        internal void Oval()
        {
            /* 周期插补点数150 */
            const int InterpNum = 150;
            /* 轨迹生成函数 */
            double[,] Trajectory = new double[6, 150];
            /* 起步数组 */
            double[,] StartData = new double[12, 75];
            /* 周期运动数组 */
            double[,] PeriodData = new double[12, 150];

            #region 插补系数
            double HipFactor_a1 = 32.26;
            double HipFactor_b1 = 0.5221;
            double HipFactor_c1 = -0.09076;
            double HipFactor_a2 = 3.178;
            double HipFactor_b2 = 1.359;
            double HipFactor_c2 = 2.301;
            double HipFactor_a3 = 0.1197;
            double HipFactor_b3 = 2.647;
            double HipFactor_c3 = 1.27;
            double HipFactor_a4 = 2.286;
            double HipFactor_b4 = 3.538;
            double HipFactor_c4 = 1.762;
            double HipFactor_a5 = 2.168;
            double HipFactor_b5 = 3.538;
            double HipFactor_c5 = 4.902;
            double HipFactor_a6 = 0.007087;
            double HipFactor_b6 = 10.32;
            double HipFactor_c6 = -12.06;
            double HipFactor_a7 = 0.01939;
            double HipFactor_b7 = 6.668;
            double HipFactor_c7 = 1.979;

            double KneeFactor_a1 = 71.42;
            double KneeFactor_b1 = 0.5121;
            double KneeFactor_c1 = -3.107;
            double KneeFactor_a2 = 2.888;
            double KneeFactor_b2 = 0.9734;
            double KneeFactor_c2 = 1.792;
            double KneeFactor_a3 = 0.3281;
            double KneeFactor_b3 = 2.094;
            double KneeFactor_c3 = -1.57;
            double KneeFactor_a4 = 0.01116;
            double KneeFactor_b4 = 3.159;
            double KneeFactor_c4 = -1.622;
            double KneeFactor_a5 = 0.007357;
            double KneeFactor_b5 = 4.087;
            double KneeFactor_c5 = -1.265;
            double KneeFactor_a6 = 0.002317;
            double KneeFactor_b6 = 5.266;
            double KneeFactor_c6 = -1.661;
            double KneeFactor_a7 = 0.0009123;
            double KneeFactor_b7 = 1.256;
            double KneeFactor_c7 = 1.571;
            #endregion

            #region 计算过程
            for (int i = 0; i < InterpNum; i++)
            {
                double time = (double)i * 6 / (double)InterpNum;  //当前时刻更新

                Trajectory[0, i] = (
                      HipFactor_a1 * Math.Sin(HipFactor_b1 * time + HipFactor_c1) + HipFactor_a2 * Math.Sin(HipFactor_b2 * time + HipFactor_c2) + HipFactor_a3 * Math.Sin(HipFactor_b3 * time + HipFactor_c3)
                    + HipFactor_a4 * Math.Sin(HipFactor_b4 * time + HipFactor_c4) + HipFactor_a5 * Math.Sin(HipFactor_b5 * time + HipFactor_c5) + HipFactor_a6 * Math.Sin(HipFactor_b6 * time + HipFactor_c6)
                    + HipFactor_a7 * Math.Sin(HipFactor_b7 * time + HipFactor_c7));
                //建立髋关节角度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[1, i] = (
                      HipFactor_b1 * HipFactor_a1 * Math.Cos(HipFactor_b1 * time + HipFactor_c1) + HipFactor_b2 * HipFactor_a2 * Math.Cos(HipFactor_b2 * time + HipFactor_c2) + HipFactor_b3 * HipFactor_a3 * Math.Cos(HipFactor_b3 * time + HipFactor_c3)
                    + HipFactor_b4 * HipFactor_a4 * Math.Cos(HipFactor_b4 * time + HipFactor_c4) + HipFactor_b5 * HipFactor_a5 * Math.Cos(HipFactor_b5 * time + HipFactor_c5) + HipFactor_b6 * HipFactor_a6 * Math.Cos(HipFactor_b6 * time + HipFactor_c6)
                    + HipFactor_b7 * HipFactor_a7 * Math.Cos(HipFactor_b7 * time + HipFactor_c7)); //建立髋关节速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[2, i] = -1.0 * (
                      HipFactor_b1 * HipFactor_b1 * HipFactor_a1 * Math.Sin(HipFactor_b1 * time + HipFactor_c1) + HipFactor_b2 * HipFactor_b2 * HipFactor_a2 * Math.Sin(HipFactor_b2 * time + HipFactor_c2) + HipFactor_b3 * HipFactor_b3 * HipFactor_a3 * Math.Sin(HipFactor_b3 * time + HipFactor_c3)
                    + HipFactor_b4 * HipFactor_b4 * HipFactor_a4 * Math.Sin(HipFactor_b4 * time + HipFactor_c4) + HipFactor_b5 * HipFactor_b5 * HipFactor_a5 * Math.Sin(HipFactor_b5 * time + HipFactor_c5) + HipFactor_b6 * HipFactor_b6 * HipFactor_a6 * Math.Sin(HipFactor_b6 * time + HipFactor_c6)
                    + HipFactor_b7 * HipFactor_b7 * HipFactor_a7 * Math.Sin(HipFactor_b7 * time + HipFactor_c7));  //建立髋关节加速度数学模型，并定时插补（插补点个数Standardgait_internumber为100） 

                Trajectory[3, i] = (
                      KneeFactor_a1 * Math.Sin(KneeFactor_b1 * time + KneeFactor_c1) + KneeFactor_a2 * Math.Sin(KneeFactor_b2 * time + KneeFactor_c2) + KneeFactor_a3 * Math.Sin(KneeFactor_b3 * time + KneeFactor_c3)
                    + KneeFactor_a4 * Math.Sin(KneeFactor_b4 * time + KneeFactor_c4) + KneeFactor_a5 * Math.Sin(KneeFactor_b5 * time + KneeFactor_c5) + KneeFactor_a6 * Math.Sin(KneeFactor_b6 * time + KneeFactor_c6)
                    + KneeFactor_a7 * Math.Sin(KneeFactor_b7 * time + KneeFactor_c7));
                //建立膝关节角度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[4, i] = (
                      KneeFactor_b1 * KneeFactor_a1 * Math.Cos(KneeFactor_b1 * time + KneeFactor_c1) + KneeFactor_b2 * KneeFactor_a2 * Math.Cos(KneeFactor_b2 * time + KneeFactor_c2) + KneeFactor_b3 * KneeFactor_a3 * Math.Cos(KneeFactor_b3 * time + KneeFactor_c3)
                    + KneeFactor_b4 * KneeFactor_a4 * Math.Cos(KneeFactor_b4 * time + KneeFactor_c4) + KneeFactor_b5 * KneeFactor_a5 * Math.Cos(KneeFactor_b5 * time + KneeFactor_c5) + KneeFactor_b6 * KneeFactor_a6 * Math.Cos(KneeFactor_b6 * time + KneeFactor_c6)
                    + KneeFactor_b7 * KneeFactor_a7 * Math.Cos(KneeFactor_b7 * time + KneeFactor_c7));  //建立膝关节速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[5, i] = -1.0 * (
                      KneeFactor_b1 * KneeFactor_b1 * KneeFactor_a1 * Math.Sin(KneeFactor_b1 * time + KneeFactor_c1) + KneeFactor_b2 * KneeFactor_b2 * KneeFactor_a2 * Math.Sin(KneeFactor_b2 * time + KneeFactor_c2) + KneeFactor_b3 * KneeFactor_b3 * KneeFactor_a3 * Math.Sin(KneeFactor_b3 * time + KneeFactor_c3)
                    + KneeFactor_b4 * KneeFactor_b4 * KneeFactor_a4 * Math.Sin(KneeFactor_b4 * time + KneeFactor_c4) + KneeFactor_b5 * KneeFactor_b5 * KneeFactor_a5 * Math.Sin(KneeFactor_b5 * time + KneeFactor_c5) + KneeFactor_b6 * KneeFactor_b6 * KneeFactor_a6 * Math.Sin(KneeFactor_b6 * time + KneeFactor_c6)
                    + KneeFactor_b7 * KneeFactor_b7 * KneeFactor_a7 * Math.Sin(KneeFactor_b7 * time + KneeFactor_c7));   //建立膝关节加速度数学模型，并定时插补（插补点个数Standardgait_internumber为100） 
            }

            for (int j = 0; j < 75; j++)
            {

                StartData[0, j] = 0;//左髋
                StartData[1, j] = 0;
                StartData[2, j] = 0;
                StartData[3, j] = Trajectory[0, j];//右髋
                StartData[4, j] = Trajectory[1, j];
                StartData[5, j] = Trajectory[2, j];
                StartData[6, j] = 0;//左膝
                StartData[7, j] = 0;
                StartData[8, j] = 0;
                if (Trajectory[3, j] > 0)
                {
                    StartData[9, j] = 0;
                }
                else
                {
                    StartData[9, j] = Trajectory[3, j];//右膝
                }
                StartData[10, j] = Trajectory[4, j];
                StartData[11, j] = Trajectory[5, j];
            }

            for (int k = 0; k < InterpNum; k++)
            {
                PeriodData[0, k] = Trajectory[0, k];
                PeriodData[1, k] = Trajectory[1, k];
                PeriodData[2, k] = Trajectory[2, k];
                if (Trajectory[3, k] > 0)
                {
                    PeriodData[6, k] = 0;
                }
                else
                {
                    PeriodData[6, k] = Trajectory[3, k];
                }
                PeriodData[7, k] = Trajectory[4, k];
                PeriodData[8, k] = Trajectory[5, k];

                if (k < 75)
                {
                    PeriodData[3, k] = Trajectory[0, k + 75];
                    PeriodData[4, k] = Trajectory[1, k + 75];
                    PeriodData[5, k] = Trajectory[2, k + 75];
                    if (Trajectory[3, k + 75] > 0)
                    {
                        PeriodData[9, k] = 0;
                    }
                    else
                    {
                        PeriodData[9, k] = Trajectory[3, k + 75];
                    }
                    PeriodData[10, k] = Trajectory[4, k + 75];
                    PeriodData[11, k] = Trajectory[5, k + 75];
                }
                else
                {
                    PeriodData[3, k] = Trajectory[0, k - 75];
                    PeriodData[4, k] = Trajectory[1, k - 75];
                    PeriodData[5, k] = Trajectory[2, k - 75];
                    if (Trajectory[3, k - 75] > 0)
                    {
                        PeriodData[9, k] = 0;
                    }
                    else
                    {
                        PeriodData[9, k] = Trajectory[3, k - 75];
                    }

                    PeriodData[10, k] = Trajectory[4, k - 75];
                    PeriodData[11, k] = Trajectory[5, k - 75];
                }
            }
            #endregion

            #region  打印到txt
            string path = @"..\..\..\JiontTxt\Oval.txt";
            File.Delete(path);
            FileStream input = new FileStream(path, FileMode.Append);
            StreamWriter output = null;
            output = new StreamWriter(input);
            output.WriteLine("阶段" + "，" + "左髋" + "，" + "右髋" + "，" + "左膝" + "，" + "右膝" + "，" + "速度");
            #endregion

            #region 轨迹生成
            for (int i = 0; i < 75; i++)
            {
                if (i < 6)
                {
                    OvalStartVel[i] = 1 + i * 1.6;
                }
                else
                {
                    OvalStartVel[i] = (float)Math.Sqrt(
                    StartData[1, i] * StartData[1, i]
                  + StartData[4, i] * StartData[4, i]
                  + StartData[7, i] * StartData[7, i]
                  + StartData[10, i] * StartData[10, i]
                  ) / 5;//50为减速比
                }

                for (int j = 0; j < 4; j++)
                {
                    OvalStartData[j, i] = StartData[3 * j, i];
                }
                output.WriteLine("起步{0}" + "，" + OvalStartData[0, i].ToString("F4") + "，" +
                    OvalStartData[1, i].ToString("F4") + "，" + OvalStartData[2, i].ToString("F4") + "，" +
                    OvalStartData[3, i].ToString("F4") + "，" + OvalStartVel[i].ToString("F4"), i);
            }

            for (int i = 0; i < 150; i++)
            {
                if (i < 6)
                {
                    OvalPeriodVel[i] = 1 + i * 1.6;
                }
                else if ((i > 69) && (i < 76))
                {
                    OvalPeriodVel[i] = 9 - 1.6 * (i - 70);
                }
                else if ((i >= 76) && (i < 82))
                {
                    OvalPeriodVel[i] = 1.6 * (i - 76) + 1;
                }
                else if ((i > 144) && (i < 150))
                {
                    OvalPeriodVel[i] = 8 - 1.6 * (i - 144);
                }
                else
                {
                    OvalPeriodVel[i] = (float)Math.Sqrt(
                 PeriodData[1, i] * PeriodData[1, i]
               + PeriodData[4, i] * PeriodData[4, i]
               + PeriodData[7, i] * PeriodData[7, i]
               + PeriodData[10, i] * PeriodData[10, i]
               ) / 5;//50为减速比
                }

                for (int j = 0; j < 4; j++)
                {
                    //标准步态角度整理
                    OvalPeriodData[j, i] = PeriodData[3 * j, i];
                }
                output.WriteLine("周期{0}" + "，" + OvalPeriodData[0, i].ToString("F4") + "，" +
                    OvalPeriodData[1, i].ToString("F4") + "，" + OvalPeriodData[2, i].ToString("F4") + "，" +
                    OvalPeriodData[3, i].ToString("F4") + "，" + OvalPeriodVel[i].ToString("F4"), i);
            }
            #endregion
        }
        #endregion

        #region 圆周训练模式
        #region 圆周训练模式数组声明
        internal double[,] CircularStartData = new double[4, 75];
        internal double[,] CircularPeriodData = new double[4, 150];
        internal double[] CircularStartVel = new double[75];
        internal double[] CircularPeriodVel = new double[150];
        #endregion

        internal void Circular()
        {
            /* 周期插补点数150 */
            const int InterpNum = 150;
            /* 轨迹生成函数 */
            double[,] Trajectory = new double[6, 150];
            /* 起步数组 */
            double[,] StartData = new double[12, 75];
            /* 周期运动数组 */
            double[,] PeriodData = new double[12, 150];

            #region 拟合关节系数
            double HipFactor_a1 = 31.25;
            double HipFactor_b1 = 0.5139;
            double HipFactor_c1 = 0.1631;
            double HipFactor_a2 = 9.408;
            double HipFactor_b2 = 0.865;
            double HipFactor_c2 = 3.819;
            double HipFactor_a3 = 0.8909;
            double HipFactor_b3 = 1.8;
            double HipFactor_c3 = 1.146;
            double HipFactor_a4 = 0.03495;
            double HipFactor_b4 = 3.238;
            double HipFactor_c4 = 2.892;
            double HipFactor_a5 = 0.0001952;
            double HipFactor_b5 = 7.17;
            double HipFactor_c5 = -6.304;

            double KneeFactor_a1 = 72.77;
            double KneeFactor_b1 = 0.5204;
            double KneeFactor_c1 = 3.151;
            double KneeFactor_a2 = 1.528;
            double KneeFactor_b2 = 1.323;
            double KneeFactor_c2 = 0.7422;
            double KneeFactor_a3 = 0.3283;
            double KneeFactor_b3 = 2.104;
            double KneeFactor_c3 = -1.601;
            double KneeFactor_a4 = -0.0007831;
            double KneeFactor_b4 = 3.026;
            double KneeFactor_c4 = -1.225;
            double KneeFactor_a5 = 0.007567;
            double KneeFactor_b5 = 3.776;
            double KneeFactor_c5 = -0.3316;
            double KneeFactor_a6 = 0.0006097;
            double KneeFactor_b6 = 5.322;
            double KneeFactor_c6 = -1.829;
            double KneeFactor_a7 = 0.0003792;
            double KneeFactor_b7 = 6.221;
            double KneeFactor_c7 = -1.383;
            #endregion

            #region 计算过程
            for (int i = 0; i < InterpNum; i++)
            {
                double time = (double)i * 6 / (double)InterpNum;  //当前时刻更新
                Trajectory[0, i] = (
                      HipFactor_a1 * Math.Sin(HipFactor_b1 * time + HipFactor_c1) + HipFactor_a2 * Math.Sin(HipFactor_b2 * time + HipFactor_c2) + HipFactor_a3 * Math.Sin(HipFactor_b3 * time + HipFactor_c3)
                    + HipFactor_a4 * Math.Sin(HipFactor_b4 * time + HipFactor_c4) + HipFactor_a5 * Math.Sin(HipFactor_b5 * time + HipFactor_c5));
                //建立髋关节角度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[1, i] = (
                      HipFactor_b1 * HipFactor_a1 * Math.Cos(HipFactor_b1 * time + HipFactor_c1) + HipFactor_b2 * HipFactor_a2 * Math.Cos(HipFactor_b2 * time + HipFactor_c2) + HipFactor_b3 * HipFactor_a3 * Math.Cos(HipFactor_b3 * time + HipFactor_c3)
                    + HipFactor_b4 * HipFactor_a4 * Math.Cos(HipFactor_b4 * time + HipFactor_c4) + HipFactor_b5 * HipFactor_a5 * Math.Cos(HipFactor_b5 * time + HipFactor_c5)); //建立髋关节速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[2, i] = -1.0 * (
                      HipFactor_b1 * HipFactor_b1 * HipFactor_a1 * Math.Sin(HipFactor_b1 * time + HipFactor_c1) + HipFactor_b2 * HipFactor_b2 * HipFactor_a2 * Math.Sin(HipFactor_b2 * time + HipFactor_c2) + HipFactor_b3 * HipFactor_b3 * HipFactor_a3 * Math.Sin(HipFactor_b3 * time + HipFactor_c3)
                    + HipFactor_b4 * HipFactor_b4 * HipFactor_a4 * Math.Sin(HipFactor_b4 * time + HipFactor_c4) + HipFactor_b5 * HipFactor_b5 * HipFactor_a5 * Math.Sin(HipFactor_b5 * time + HipFactor_c5));  //建立髋关节加速度数学模型，并定时插补（插补点个数Standardgait_internumber为100） 

                Trajectory[3, i] = (
                      KneeFactor_a1 * Math.Sin(KneeFactor_b1 * time + KneeFactor_c1) + KneeFactor_a2 * Math.Sin(KneeFactor_b2 * time + KneeFactor_c2) + KneeFactor_a3 * Math.Sin(KneeFactor_b3 * time + KneeFactor_c3)
                    + KneeFactor_a4 * Math.Sin(KneeFactor_b4 * time + KneeFactor_c4) + KneeFactor_a5 * Math.Sin(KneeFactor_b5 * time + KneeFactor_c5) + KneeFactor_a6 * Math.Sin(KneeFactor_b6 * time + KneeFactor_c6)
                    + KneeFactor_a7 * Math.Sin(KneeFactor_b7 * time + KneeFactor_c7));
                //建立膝关节角度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[4, i] = (
                      KneeFactor_b1 * KneeFactor_a1 * Math.Cos(KneeFactor_b1 * time + KneeFactor_c1) + KneeFactor_b2 * KneeFactor_a2 * Math.Cos(KneeFactor_b2 * time + KneeFactor_c2) + KneeFactor_b3 * KneeFactor_a3 * Math.Cos(KneeFactor_b3 * time + KneeFactor_c3)
                    + KneeFactor_b4 * KneeFactor_a4 * Math.Cos(KneeFactor_b4 * time + KneeFactor_c4) + KneeFactor_b5 * KneeFactor_a5 * Math.Cos(KneeFactor_b5 * time + KneeFactor_c5) + KneeFactor_b6 * KneeFactor_a6 * Math.Cos(KneeFactor_b6 * time + KneeFactor_c6)
                    + KneeFactor_b7 * KneeFactor_a7 * Math.Cos(KneeFactor_b7 * time + KneeFactor_c7));  //建立膝关节速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[5, i] = -1.0 * (
                      KneeFactor_b1 * KneeFactor_b1 * KneeFactor_a1 * Math.Sin(KneeFactor_b1 * time + KneeFactor_c1) + KneeFactor_b2 * KneeFactor_b2 * KneeFactor_a2 * Math.Sin(KneeFactor_b2 * time + KneeFactor_c2) + KneeFactor_b3 * KneeFactor_b3 * KneeFactor_a3 * Math.Sin(KneeFactor_b3 * time + KneeFactor_c3)
                    + KneeFactor_b4 * KneeFactor_b4 * KneeFactor_a4 * Math.Sin(KneeFactor_b4 * time + KneeFactor_c4) + KneeFactor_b5 * KneeFactor_b5 * KneeFactor_a5 * Math.Sin(KneeFactor_b5 * time + KneeFactor_c5) + KneeFactor_b6 * KneeFactor_b6 * KneeFactor_a6 * Math.Sin(KneeFactor_b6 * time + KneeFactor_c6)
                    + KneeFactor_b7 * KneeFactor_b7 * KneeFactor_a7 * Math.Sin(KneeFactor_b7 * time + KneeFactor_c7));   //建立膝关节加速度数学模型，并定时插补（插补点个数Standardgait_internumber为100） 
            }

            for (int j = 0; j < 75; j++)
            {

                StartData[0, j] = 0;//左髋
                StartData[1, j] = 0;
                StartData[2, j] = 0;
                StartData[3, j] = Trajectory[0, j];//右髋
                StartData[4, j] = Trajectory[1, j];
                StartData[5, j] = Trajectory[2, j];
                StartData[6, j] = 0;//左膝
                StartData[7, j] = 0;
                StartData[8, j] = 0;
                if (Trajectory[3, j] > 0)
                {
                    StartData[9, j] = 0;
                }
                else
                {
                    StartData[9, j] = Trajectory[3, j];//右膝
                }
                StartData[10, j] = Trajectory[4, j];
                StartData[11, j] = Trajectory[5, j];
            }

            for (int k = 0; k < InterpNum; k++)
            {
                PeriodData[0, k] = Trajectory[0, k];
                PeriodData[1, k] = Trajectory[1, k];
                PeriodData[2, k] = Trajectory[2, k];
                if (Trajectory[3, k] > 0)
                {
                    PeriodData[6, k] = 0;
                }
                else
                {
                    PeriodData[6, k] = Trajectory[3, k];
                }
                PeriodData[7, k] = Trajectory[4, k];
                PeriodData[8, k] = Trajectory[5, k];

                if (k < 75)
                {
                    PeriodData[3, k] = Trajectory[0, k + 75];
                    PeriodData[4, k] = Trajectory[1, k + 75];
                    PeriodData[5, k] = Trajectory[2, k + 75];
                    if (Trajectory[3, k + 75] > 0)
                    {
                        PeriodData[9, k] = 0;
                    }
                    else
                    {
                        PeriodData[9, k] = Trajectory[3, k + 75];
                    }
                    PeriodData[10, k] = Trajectory[4, k + 75];
                    PeriodData[11, k] = Trajectory[5, k + 75];
                }
                else
                {
                    PeriodData[3, k] = Trajectory[0, k - 75];
                    PeriodData[4, k] = Trajectory[1, k - 75];
                    PeriodData[5, k] = Trajectory[2, k - 75];
                    if (Trajectory[3, k - 75] > 0)
                    {
                        PeriodData[9, k] = 0;
                    }
                    else
                    {
                        PeriodData[9, k] = Trajectory[3, k - 75];
                    }

                    PeriodData[10, k] = Trajectory[4, k - 75];
                    PeriodData[11, k] = Trajectory[5, k - 75];
                }
            }
            #endregion

            #region  打印到txt
            string path = @"..\..\..\JiontTxt\Circular.txt";
            File.Delete(path);
            FileStream input = new FileStream(path, FileMode.Append);
            StreamWriter output = null;
            output = new StreamWriter(input);
            output.WriteLine("阶段" + "，" + "左髋" + "，" + "右髋" + "，" + "左膝" + "，" + "右膝" + "，" + "速度");
            #endregion

            #region 轨迹生成
            for (int i = 0; i < 75; i++)
            {
                if (i < 6)
                {
                    CircularStartVel[i] = 2 + i * 1.4;
                }
                else
                {
                    CircularStartVel[i] = (float)Math.Sqrt(
                  StartData[1, i] * StartData[1, i]
                + StartData[4, i] * StartData[4, i]
                + StartData[7, i] * StartData[7, i]
                + StartData[10, i] * StartData[10, i]
                ) / 5;//50为减速比
                }

                for (int j = 0; j < 4; j++)
                {
                    CircularStartData[j, i] = StartData[3 * j, i];
                }
                output.WriteLine("起步{0}" + "，" + CircularStartData[0, i].ToString("F4") + "，" +
                    CircularStartData[1, i].ToString("F4") + "，" + CircularStartData[2, i].ToString("F4") + "，" +
                    CircularStartData[3, i].ToString("F4") + "，" + CircularStartVel[i].ToString("F4"), i);
            }

            for (int i = 0; i < 150; i++)
            {
                if (i < 6)
                {
                    CircularPeriodVel[i] = 2 + i * 1.4;
                }
                else if ((i > 69) && (i < 76))
                {
                    CircularPeriodVel[i] = 8 - 1.4 * (i - 70);
                }
                else if ((i >= 76) && (i < 82))
                {
                    CircularPeriodVel[i] = 1.4 * (i - 76) + 2;
                }
                else if ((i > 144) && (i < 150))
                {
                    CircularPeriodVel[i] = 8 - 1.4 * (i - 145);
                }
                else
                {
                    CircularPeriodVel[i] = (float)Math.Sqrt(
                  PeriodData[1, i] * PeriodData[1, i]
                + PeriodData[4, i] * PeriodData[4, i]
                + PeriodData[7, i] * PeriodData[7, i]
                + PeriodData[10, i] * PeriodData[10, i]
                ) / 5;//50为减速比
                }

                for (int j = 0; j < 4; j++)
                {
                    CircularPeriodData[j, i] = PeriodData[3 * j, i];
                }
                output.WriteLine("周期{0}" + "，" + CircularPeriodData[0, i].ToString("F4") + "，" +
                    CircularPeriodData[1, i].ToString("F4") + "，" + CircularPeriodData[2, i].ToString("F4") + "，" +
                    CircularPeriodData[3, i].ToString("F4") + "，" + CircularPeriodVel[i].ToString("F4"), i);
            }
            output.Close();
            #endregion
        }
        #endregion

        #region 髋关节训练模式
        #region 髋关节训练模式数组声明
        internal double[,] HipStartData = new double[4, 75];
        internal double[,] HipPeriodData = new double[4, 150];
        internal double[] HipStartVel = new double[37];
        internal double[] HipPeriodVel = new double[150];
        #endregion

        internal void Hip()
        {
            /* 周期插补点数150 */
            const int InterpNum = 150;
            /* 轨迹生成函数 */
            double[,] Trajectory = new double[3, 150];
            /* 起步数组 */
            double[,] StartData = new double[6, 37];
            /* 周期运动数组 */
            double[,] PeriodData = new double[6, 150];

            #region 拟合关节系数
            double Hip_a1 = 12.67;
            double Hip_b1 = 0.2416;
            double Hip_c1 = -1.843;
            double Hip_a2 = 17.13;
            double Hip_b2 = 0.05477;
            double Hip_c2 = 1.596;
            double Hip_a3 = 4.995;
            double Hip_b3 = 0.3349;
            double Hip_c3 = -1.178;
            double Hip_a4 = 1.607;
            double Hip_b4 = 0.4137;
            double Hip_c4 = -0.5923;
            double Hip_a5 = 0.5545;
            double Hip_b5 = 0.7546;
            double Hip_c5 = 0.2489;
            double Hip_a6 = 0.4584;
            double Hip_b6 = 0.7865;
            double Hip_c6 = 2.501;
            double Hip_a7 = 0.04138;
            double Hip_b7 = 1.283;
            double Hip_c7 = 2.397;
            #endregion

            #region 计算过程
            for (int i = 0; i < InterpNum; i++)
            {
                double time = (double)i * 40 / (double)InterpNum;  //当前时刻更新

                Trajectory[0, i] = (
                      Hip_a1 * Math.Sin(Hip_b1 * time + Hip_c1) + Hip_a2 * Math.Sin(Hip_b2 * time + Hip_c2) + Hip_a3 * Math.Sin(Hip_b3 * time + Hip_c3)
                    + Hip_a4 * Math.Sin(Hip_b4 * time + Hip_c4) + Hip_a5 * Math.Sin(Hip_b5 * time + Hip_c5) + Hip_a6 * Math.Sin(Hip_b6 * time + Hip_c6)
                    + Hip_a7 * Math.Sin(Hip_b7 * time + Hip_c7));//建立膝关节角度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[1, i] = (
                     Hip_b1 * Hip_a1 * Math.Cos(Hip_b1 * time + Hip_c1) + Hip_b2 * Hip_a2 * Math.Cos(Hip_b2 * time + Hip_c2) + Hip_b3 * Hip_a3 * Math.Cos(Hip_b3 * time + Hip_c3)
                   + Hip_b4 * Hip_a4 * Math.Cos(Hip_b4 * time + Hip_c4) + Hip_b5 * Hip_a5 * Math.Cos(Hip_b5 * time + Hip_c5) + Hip_b6 * Hip_a6 * Math.Cos(Hip_b6 * time + Hip_c6)
                   + Hip_b7 * Hip_a7 * Math.Cos(Hip_b7 * time + Hip_c7));  //建立膝关节速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[2, i] = -1.0 * (
                     Hip_b1 * Hip_b1 * Hip_a1 * Math.Sin(Hip_b1 * time + Hip_c1) + Hip_b2 * Hip_b2 * Hip_a2 * Math.Sin(Hip_b2 * time + Hip_c2) + Hip_b3 * Hip_b3 * Hip_a3 * Math.Sin(Hip_b3 * time + Hip_c3)
                   + Hip_b4 * Hip_b4 * Hip_a4 * Math.Sin(Hip_b4 * time + Hip_c4) + Hip_b5 * Hip_b5 * Hip_a5 * Math.Sin(Hip_b5 * time + Hip_c5) + Hip_b6 * Hip_b6 * Hip_a6 * Math.Sin(Hip_b6 * time + Hip_c6)
                   + Hip_b7 * Hip_b7 * Hip_a7 * Math.Sin(Hip_b7 * time + Hip_c7));   //建立膝关节加速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）   
            }

            for (int j = 0; j < 37; j++)
            {
                StartData[0, j] = 0;
                StartData[1, j] = 0;
                StartData[2, j] = 0;
                StartData[3, j] = Trajectory[0, j];
                StartData[4, j] = Trajectory[1, j];
                StartData[5, j] = Trajectory[2, j];
            }

            for (int k = 0; k < InterpNum; k++)
            {
                PeriodData[0, k] = Trajectory[0, k];
                PeriodData[1, k] = Trajectory[1, k];
                PeriodData[2, k] = Trajectory[2, k];
                if (k < 112)
                {
                    PeriodData[3, k] = Trajectory[0, k + 37];
                    PeriodData[4, k] = Trajectory[1, k + 37];
                    PeriodData[5, k] = Trajectory[2, k + 37];
                }
                else
                {
                    PeriodData[3, k] = Trajectory[0, k - 112];
                    PeriodData[4, k] = Trajectory[1, k - 112];
                    PeriodData[5, k] = Trajectory[2, k - 112];
                }
            }
            #endregion

            #region  打印到txt
            string path = @"..\..\..\JiontTxt\Hip.txt";
            File.Delete(path);
            FileStream input = new FileStream(path, FileMode.Append);
            StreamWriter output = null;
            output = new StreamWriter(input);
            output.WriteLine("阶段" + "，" + "左髋" + "，" + "右髋" + "，" + "速度");
            #endregion

            #region 轨迹生成
            for (int i = 0; i < 37; i++)
            {
                HipStartVel[i] = (float)Math.Sqrt(StartData[1, i] * StartData[1, i] + StartData[4, i] * StartData[4, i]) / 2.5;
                for (int j = 0; j < 2; j++)
                {
                    HipStartData[j, i] = StartData[j * 3, i];
                }
                HipStartData[2, i] = 0;
                HipStartData[3, i] = 0;
                output.WriteLine("起步{0}" + "，" + HipStartData[0, i].ToString("F4") + "，" +
                    HipStartData[1, i].ToString("F4") + "，" + HipStartVel[i].ToString("F4"), i);
            }

            for (int i = 0; i < 150; i++)
            {
                HipPeriodVel[i] = (float)Math.Sqrt(PeriodData[1, i] * PeriodData[1, i]+ PeriodData[4, i] * PeriodData[4, i]) / 2.5;
                for (int j = 0; j < 2; j++)
                {
                    HipPeriodData[j, i] = PeriodData[3*j, i];
                }
                HipPeriodData[2, i] = 0;
                HipPeriodData[3, i] = 0;
                output.WriteLine("周期{0}" + "，" + HipPeriodData[0, i].ToString("F4") + "，" +
                    HipPeriodData[1, i].ToString("F4") + "，" + HipPeriodVel[i].ToString("F4"), i);
            }
            output.Close();
            #endregion
        }
        #endregion

        #region 膝关节训练模式
        #region 膝关节训练模式数组声明
        internal double[,] KneeStartData = new double[4, 75];
        internal double[,] KneePeriodData = new double[4, 150];
        internal double[] KneeStartVel = new double[75];
        internal double[] KneePeriodVel = new double[150];
        #endregion

        internal void Knee()
        {
            /* 周期插补点数150 */
            const int InterpNum = 150;
            /* 轨迹生成函数 */
            double[,] Trajectory = new double[3, 150];
            /* 起步数组 */
            double[,] StartData = new double[6, 75];
            /* 周期运动数组 */
            double[,] PeriodData = new double[6, 150];

            #region 拟合关节系数
            double Knee_a1 = 25.38;
            double Knee_b1 = 0.05035;
            double Knee_c1 = -2.578;
            double Knee_a2 = 14.38;
            double Knee_b2 = 0.1729;
            double Knee_c2 = 1.256;
            double Knee_a3 = 0.01399;
            double Knee_b3 = 0.237;
            double Knee_c3 = -0.6114;
            double Knee_a4 = -0.1149;
            double Knee_b4 = 0.5595;
            double Knee_c4 = -9.62;
            double Knee_a5 = 0.08702;
            double Knee_b5 = 0.4393;
            double Knee_c5 = 2.193;
            double Knee_a6 = -0.008172;
            double Knee_b6 = 0.7015;
            double Knee_c6 = 0.09025;
            double Knee_a7 = 0.02285;
            double Knee_b7 = 0.9344;
            double Knee_c7 = -1.427;
            #endregion

            #region 计算过程
            /* 首先先计算插补点信息 */
            for (int i = 0; i < InterpNum; i++)
            {
                double time = (double)i * 40 / (double)InterpNum;  //当前时刻更新
                Trajectory[0, i] = (
                      Knee_a1 * Math.Sin(Knee_b1 *time + Knee_c1) + Knee_a2 * Math.Sin(Knee_b2 *time + Knee_c2) + Knee_a3 * Math.Sin(Knee_b3 *time + Knee_c3)
                    + Knee_a4 * Math.Sin(Knee_b4 *time + Knee_c4) + Knee_a5 * Math.Sin(Knee_b5 *time + Knee_c5) + Knee_a6 * Math.Sin(Knee_b6 *time + Knee_c6)
                    + Knee_a7 * Math.Sin(Knee_b7 *time + Knee_c7));//建立膝关节角度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[1, i] = (
                     Knee_b1 *Knee_a1 * Math.Cos(Knee_b1 *time + Knee_c1) + Knee_b2 *Knee_a2 * Math.Cos(Knee_b2 *time + Knee_c2) + Knee_b3 *Knee_a3 * Math.Cos(Knee_b3 *time + Knee_c3)
                   + Knee_b4 *Knee_a4 * Math.Cos(Knee_b4 *time + Knee_c4) + Knee_b5 *Knee_a5 * Math.Cos(Knee_b5 *time + Knee_c5) + Knee_b6 *Knee_a6 * Math.Cos(Knee_b6 *time + Knee_c6)
                   + Knee_b7 *Knee_a7 * Math.Cos(Knee_b7 *time + Knee_c7));  //建立膝关节速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）

                Trajectory[2, i] = -1.0 * (
                     Knee_b1 * Knee_b1 *Knee_a1 * Math.Sin(Knee_b1 *time + Knee_c1) + Knee_b2 * Knee_b2 *Knee_a2 * Math.Sin(Knee_b2 *time + Knee_c2) + Knee_b3 * Knee_b3 *Knee_a3 * Math.Sin(Knee_b3 *time + Knee_c3)
                   + Knee_b4 * Knee_b4 *Knee_a4 * Math.Sin(Knee_b4 *time + Knee_c4) + Knee_b5 * Knee_b5 *Knee_a5 * Math.Sin(Knee_b5 *time + Knee_c5) + Knee_b6 * Knee_b6 *Knee_a6 * Math.Sin(Knee_b6 *time + Knee_c6)
                   + Knee_b7 * Knee_b7 *Knee_a7 * Math.Sin(Knee_b7 *time + Knee_c7));   //建立膝关节加速度数学模型，并定时插补（插补点个数Standardgait_internumber为100）   
            }

            /* 对插补点信息进行规划 */
            for (int j = 0; j < InterpNum / 2; j++)
            {
                StartData[0, j] = 0;
                StartData[1, j] = 0;
                StartData[2, j] = 0;
                StartData[3, j] = Trajectory[0, j];
                StartData[4, j] = Trajectory[1, j];
                StartData[5, j] = Trajectory[2, j];
            }

            for (int k = 0; k < InterpNum; k++)
            {
                if (k < InterpNum / 2)
                {
                    PeriodData[0, k] = Trajectory[0, k];
                    PeriodData[1, k] = Trajectory[1, k];
                    PeriodData[2, k] = Trajectory[2, k];
                    PeriodData[3, k] = Trajectory[0, k + InterpNum / 2];
                    PeriodData[4, k] = Trajectory[1, k + InterpNum / 2];
                    PeriodData[5, k] = Trajectory[2, k + InterpNum / 2];
                }
                else
                {
                    PeriodData[0, k] = Trajectory[0, k];
                    PeriodData[1, k] = Trajectory[1, k];
                    PeriodData[2, k] = Trajectory[2, k];
                    PeriodData[3, k] = Trajectory[0, k - InterpNum / 2];
                    PeriodData[4, k] = Trajectory[1, k - InterpNum / 2];
                    PeriodData[5, k] = Trajectory[2, k - InterpNum / 2];
                }
            }
            #endregion

            #region  打印到txt
            string path = @"..\..\..\JiontTxt\Knee.txt";
            File.Delete(path);
            FileStream input = new FileStream(path, FileMode.Append);
            StreamWriter output = null;
            output = new StreamWriter(input);
            output.WriteLine("阶段" + "，" + "左膝" + "，" + "右膝" + "，" + "速度");
            #endregion

            #region 轨迹生成
            for (int i = 0; i < 75; i++)
            {
                KneeStartVel[i] = (float)Math.Sqrt(StartData[1, i] * StartData[1, i] + StartData[4, i] * StartData[4, i]) / 1.5;
                KneeStartData[0, i] = 0;
                KneeStartData[1, i] = 0;
                for (int j = 0; j < 2; j++)
                {
                    KneeStartData[j+2, i] = StartData[j * 3, i];
                }               
                output.WriteLine("起步{0}" + "，" + KneeStartData[2, i].ToString("F4") + "，" +
                    KneeStartData[3, i].ToString("F4") + "，" + KneeStartVel[i].ToString("F4"), i);
            }

            for (int i = 0; i < 150; i++)
            {
                KneePeriodVel[i] = (float)Math.Sqrt(PeriodData[1, i] * PeriodData[1, i] + PeriodData[4, i] * PeriodData[4, i]) / 1.5;
                KneePeriodData[0, i] = 0;
                KneePeriodData[1, i] = 0;
                for (int j = 0; j < 2; j++)
                {
                    KneePeriodData[j+2, i] = PeriodData[3 * j, i];
                }               
                output.WriteLine("周期{0}" + "，" + KneePeriodData[2, i].ToString("F4") + "，" +
                    KneePeriodData[3, i].ToString("F4") + "，" + KneePeriodVel[i].ToString("F4"), i);
            }
            output.Close();
            #endregion
        }
        #endregion
    }
}                     

